#include "QuadTree.h"
#include <list>
#include <deque>
#include <iostream>
#include <cilk/cilk.h>

#define NODE_SIZE 16
#define MAX_DEPTH 5

QuadTree::QuadTree(vector<Line*>* lines, double min_x, double max_x, double min_y, double max_y) {
  //Create the head node in the tree
  head = new QtNode;
 
  //Set relevant pointers
  head->parent = NULL;
  head->children = NULL;
  
  //Initialize the set of lines
  head->lines.assign(lines->begin(), lines->end());
  
  //Set the bounds
  head->min_x = min_x;
  head->max_x = max_x;
  head->min_y = min_y;
  head->max_y = max_y;
  
  //Set our initital depth
  head->depth = 1;
  
  //Start the recursive processing of the quadtree
  __recurseNodes(head);
}

//Destructor, recursively destroy children allocations
QuadTree::~QuadTree() {
  if (head != NULL) {
    __deleteRecurse(head);
	delete head;
  }
}

//Recursively function for destroying children allocations
void QuadTree::__deleteRecurse(QtNode* node) {
  //Call recursive delete on children
  if (node->children != NULL) {
    for (int i = 0; i < 4; i++) {
	  __deleteRecurse(&(node->children[i]));
    }
	//Delete children
	delete[] node->children;
  }
}

//Check for collisions by traversing down the tree
//ARGS
//   node: Current QtNode that we are considering
//   dagNode: Head of our section of the DAG. This is extended when calling children
//   world: Pointer to the world as a whole, for getting necessary information 
void QuadTree::__checkCollisionsRecurse(QtNode *node, DagNode *dagNode, CollisionWorld *world) {
   //Check all lines in our scope of comparison
   //*Among themselves
   list<Line*>::iterator it1, it2;
   
   for (it1 = node->lines.begin(); it1 != node->lines.end(); ++it1) {
      Line *l1 = *it1;
	  it2 = it1;
	  it2++;
      for (/*it2 = it1 + 1*/; it2 != node->lines.end(); ++it2) {
         Line *l2 = *it2;
         IntersectionType intersectionType = intersect(l1, l2, world->timeStep);
         if (intersectionType != NO_INTERSECTION) {
            world->collisionSolver(l1, l2, intersectionType);
            world->numLineLineCollisions++;
         }
      }
   }
   
   deque<Line*>::iterator it3;
   
   //*Among parents and grandparents
   for (it1 = node->lines.begin(); it1 != node->lines.end(); ++it1) {
      Line *l1 = *it1;
	  
	  //Traverse up the DAG from our position
	  DagNode *cur = dagNode;
	  while (cur != NULL) { //Loop until at root of DAG
         Line *l2 = cur->line;
		 
         IntersectionType intersectionType = intersect(l1, l2, world->timeStep);
         if (intersectionType != NO_INTERSECTION) {
		    this->intersections.push_back(IntersectionInfo(l1, l2, intersectionType));
            world->numLineLineCollisions++;
         }
	  
	     cur = cur->next;
	  }
   }
   
   //If not a leaf node, recurse deeper, otherwise we're done
   if (node->children != NULL) {
      //Extend our portion of the DAG with own lines before calling children
	  DagNode *newDagNode = dagNode;
	  
      for (it1 = node->lines.begin(); it1 != node->lines.end(); ++it1) {
         Line *l1 = *it1;
		 newDagNode = new DagNode(newDagNode, l1);
      }
	  
	  //Call children
      for (int i = 0; i < 4; i++) {
	     if (node->children[i].lines.size() != 0) { //Don't waste time with spawning empty calls
		    cilk_spawn __checkCollisionsRecurse(&(node->children[i]), newDagNode, world);
		 }
      }
	  
	  cilk_sync;
	  
	  //Clear our extension off the queue
	  DagNode *curNode = newDagNode;
	  DagNode *nextNode = NULL;
	  for (int i = 0; i < node->lines.size(); i++) {
         nextNode = curNode->next;
		 delete curNode;
		 curNode = nextNode;
	  }
   }
}

//Check for collisions.
void QuadTree::checkCollisions(CollisionWorld *world) {
   //Inititalize the queue used in recursion
   deque<Line*> queue;
   
   //Check for collisions recurisvely
   __checkCollisionsRecurse(this->head, NULL, world);
   
   //Solve for all the queued-up collisions   
   list<IntersectionInfo> r = this->intersections.get_value();

   //Todo: Split this up recursively
   for (list<IntersectionInfo>::iterator it1 = r.begin(); it1 != r.end(); ++it1)
   {
      IntersectionInfo intersect = *it1;
      world->collisionSolver(intersect.l1, intersect.l2, intersect.intersectionType);
   }

}

//Recursive function building the quadtree from the base components
void QuadTree::__recurseNodes(QtNode* node) {
  //Check how many elements are in this node; return if no need to split further
  if (node->lines.size() <= NODE_SIZE || node->depth >= MAX_DEPTH ) {
    //if (node->lines.size() > 0 ) { cout << "Ending at size=" << node->lines.size() << " at depth=" << node->depth << endl; }
    return;
  }
  
  //Calculate boundaries
  double midX = (node->min_x + node->max_x)/2.;
  double midY = (node->min_y + node->max_y)/2.;
  
  //Create the children nodes
  node->children = new QtNode[4];
  
  //Update the children's parent and children pointer
  for (int i = 0; i < 4; i++) {
    node->children[i].parent = node;
	node->children[i].children = NULL;
	node->children[i].depth = node->depth + 1;
  }
  
  //Set the children's ranges
  node->children[0].min_x = midX;
  node->children[0].max_x = node->max_x;
  node->children[0].min_y = node->min_y;
  node->children[0].max_y = midY;
  
  node->children[1].min_x = node->min_x;
  node->children[1].max_x = midX;
  node->children[1].min_y = node->min_y;
  node->children[1].max_y = midY;
  
  node->children[2].min_x = node->min_x;
  node->children[2].max_x = midX;
  node->children[2].min_y = midY;
  node->children[2].max_y = node->max_y;
  
  node->children[3].min_x = midX;
  node->children[3].max_x = node->max_x;
  node->children[3].min_y = midY;
  node->children[3].max_y = node->max_y;
  
  //Split up the lines among children; keep ones that fall in multiple regions here
  list<Line*>::iterator it = node->lines.begin();
  
  while (it != node->lines.end())
  {
    bool removeLine = false;
	
	if ( (*it)->p1.x < midX && (*it)->p2.x < midX ) { //Left half
	  if ((*it)->p1.y < midY && (*it)->p2.y < midY) { //Top half (Quadrant II)
	    node->children[1].lines.push_back(*it);
		removeLine = true;
	  } else if ((*it)->p1.y >= midY && (*it)->p2.y >= midY) { //Bottom half (Quadrant III)
	    node->children[2].lines.push_back(*it);
		removeLine = true;
	  }
	} else if ( (*it)->p1.x >= midX && (*it)->p2.x >= midX ) { //Right half
	  if ((*it)->p1.y < midY && (*it)->p2.y < midY) { //Top half (Quadrant I)
	    node->children[0].lines.push_back(*it);
		removeLine = true;
	  } else if ((*it)->p1.y >= midY && (*it)->p2.y >= midY) { //Bottom half (Quadrant IV)
	    node->children[3].lines.push_back(*it);
		removeLine = true;
	  }
	}
	
	//Delete line from this node if a child is taking it
	if (removeLine) {
	  it = node->lines.erase(it); //Remove from current list and get a new good iterator
	} else { //Otherwise move onto the next one and leave it here
	  ++it;
	}
  }
  
  //Recurse to the child nodes
  for (int i = 0; i < 4; i++) {
	 __recurseNodes(&node->children[i]);
  }
}